/*
 * FaceDetect.cpp
 *
 *  Created on: Nov 10, 2012
 *      Author: heinstein
 */

#include "FaceDetect.hpp"
#include "Face.hpp"
#include "FaceList.hpp"
#include <iostream>
#include <stdlib.h>
#include <algorithm>

#define DOWN_SCALE 2
#define CCD_CASCADENAME "/usr/local/share/OpenCV/haarcascades/haarcascade_frontalface_alt.xml"


using namespace cv;


FaceDetect::FaceDetect(){
	uIdCounter=0;
	videoStream.open(0);
	//videoStream.set( CV_CAP_PROP_FPS, 15.0 );
	videoStream.set( CV_CAP_PROP_FRAME_WIDTH, 640.0 );
	videoStream.set( CV_CAP_PROP_FRAME_HEIGHT, 480.0 );


	//	CV_CAP_PROP_POS_MSEC Current position of the video file in milliseconds.
	//	CV_CAP_PROP_POS_FRAMES 0-based index of the frame to be decoded/captured next.
	//	CV_CAP_PROP_POS_AVI_RATIO Relative position of the video file: 0 - start of the film, 1 - end of the film.
	//	CV_CAP_PROP_FRAME_WIDTH Width of the frames in the video stream.
	//	CV_CAP_PROP_FRAME_HEIGHT Height of the frames in the video stream.
	//	CV_CAP_PROP_FPS Frame rate.
	//	CV_CAP_PROP_FOURCC 4-character code of codec.
	//	CV_CAP_PROP_FRAME_COUNT Number of frames in the video file.
	//	CV_CAP_PROP_FORMAT Format of the Mat objects returned by retrieve() .
	//	CV_CAP_PROP_MODE Backend-specific value indicating the current capture mode.
	//	CV_CAP_PROP_BRIGHTNESS Brightness of the image (only for cameras).
	//	CV_CAP_PROP_CONTRAST Contrast of the image (only for cameras).
	//	CV_CAP_PROP_SATURATION Saturation of the image (only for cameras).
	//	CV_CAP_PROP_HUE Hue of the image (only for cameras).
	//	CV_CAP_PROP_GAIN Gain of the image (only for cameras).
	//	CV_CAP_PROP_EXPOSURE Exposure (only for cameras).
	//	CV_CAP_PROP_CONVERT_RGB Boolean flags indicating whether images should be converted to RGB.
	//	CV_CAP_PROP_WHITE_BALANCE Currently unsupported
	//	CV_CAP_PROP_RECTIFICATION Rectification flag for stereo cameras (note: only supported by DC1394 v 2.x backend currently)


	if(!videoStream.isOpened())
		throw "Failed to open videoStream";
	if( !cascade.load( CCD_CASCADENAME ))
		throw "Error: Couldn't load cascade(s)!";
}

FaceDetect::~FaceDetect() {
	videoStream.release();
}

//Public function to retrieve a frame and progress it.
void FaceDetect::Detect(void) {

	vector<Rect> facesVector;
	double detectionTimeTemp;
	static int count=0;

	// Capture Frame
	Mat frame;
	videoStream.read( frame );
	if( frame.empty() )
		throw "Failed to capture frame";
	//std::cout << "Size:" <<frame.cols<<"x"<<frame.rows<< std::endl;

	// Gray-tone image and resize
	Mat grayFrame, smallFrame( cvRound (frame.rows/DOWN_SCALE), cvRound(frame.cols/DOWN_SCALE), CV_8UC1 );
	cvtColor( frame, grayFrame, CV_BGR2GRAY );
	resize( grayFrame, smallFrame, smallFrame.size(), 0, 0, INTER_LINEAR );
	equalizeHist( smallFrame, smallFrame );

	count++;
	count %=3;
	// Detect and put them in the list facesVector!
	if (!count){
		detectionTimeTemp = (double)cvGetTickCount();
		cascade.detectMultiScale( smallFrame, facesVector,
				1.05, 2, 0
				//|CV_HAAR_FIND_BIGGEST_OBJECT
				//|CV_HAAR_DO_ROUGH_SEARCH
				|CV_HAAR_SCALE_IMAGE
				,
				Size(40, 40) );
		detectionTime = (double)cvGetTickCount() - detectionTimeTemp;


		//Track the detected faces.
		std::vector<Face> fVector;
		for (uint i=0;i<facesVector.size();i++)
			fVector.push_back( getFaceRect(&frame,&(facesVector.at(i))) );
		FaceTrack(fVector);
	}
	// Show the faces in a fancy window.
	showFaces(inFrame,frame);

	for(int i = 0;i<outOfFrame.size();i++){
		if(outOfFrame.at(i)->saveToDisk())
		{
			outOfFrame.erase(outOfFrame.begin()+i);
			std::cout << "Deleted element: " << i << std::endl;
		}
	}

	// Wait and give system time to do other stuff.
	waitKey(10);
}

// Internal helper function to create a Face/calls object from an an rectangle and rawimage.
Face FaceDetect::getFaceRect(cv::Mat *rawFrame, cv::Rect *rect){

	cv::Rect r2;
	double oscalex=0.00;
	double oscaley=0.21739130434;
	cv::Mat outputFrame;

	double x,y;
	double diagonal;

	r2.x = rect->x*DOWN_SCALE - rect->width*oscalex*DOWN_SCALE;
	if (r2.x<0) {r2.x=0;};if (r2.x>rawFrame->cols) {r2.x=rawFrame->cols;};

	r2.y = rect->y*DOWN_SCALE - rect->height*oscaley*DOWN_SCALE;
	if (r2.y<0) {r2.y=0;};if (r2.y>rawFrame->rows) {r2.y=rawFrame->rows;};

	r2.width =  rect->width*DOWN_SCALE + 2*rect->width*oscalex*DOWN_SCALE;
	if (r2.width+r2.x>rawFrame->cols) {r2.width=rawFrame->cols-r2.x;};

	r2.height= rect->height*DOWN_SCALE + 2*rect->height*oscaley*DOWN_SCALE;
	if (r2.height+r2.y>rawFrame->rows) {r2.height=rawFrame->rows-r2.y;};

	x=r2.x+r2.width/2;
	y=r2.y+r2.height/2;

	diagonal=sqrt(r2.width*r2.width+r2.height*r2.height)/2;

	cvtColor( (*rawFrame)(r2), outputFrame, CV_BGR2GRAY );

	Face face=Face(Point(x,y),diagonal,outputFrame);

	return face;
}

// Internal helper function to keep track of all the recent detected faces
// Its keeps list's of faces that in size an position suggest that these is the same person.
void FaceDetect::FaceTrack(std::vector<Face> faces){

	//showFaces2(faces);

	bool found;

	for (uint i=0;i<faces.size();i++){ //loop over input faces (forbryder)
		found=false;
		//std::cout << "i:" << i <<",Size faces:"<<faces.size()<<std::endl;
		for (uint u=0;u<inFrame.size();u++){ //loop over stored faces (forbryder album)
			//std::cout  <<"   u:"<<u << ",Size inFrame:"<<inFrame.size()<<std::endl;
			if (inFrame.at(u)->getFaces().back().Compare(faces.at(i)) && !found){
				inFrame.at(u)->putFace(faces.at(i));
				inFrame.at(u)->resetUnSeenCount();
				found=true;
				//std::cout << "      Found i:" << i <<",u:"<<u <<std::endl;
			}
		}
		if (!found){
			inFrame.push_back(new FaceList(uIdCounter++));
			inFrame.back()->putFace(faces.at(i));
			//std::cout << "   New   i:" << i << std::endl;
		}
	}

	//Clean up the inFrame-list and push the list's that is big enough or has run for long to the outOfFrame-list
	for (uint u=0;u<inFrame.size();u++){

		if (inFrame.at(u)->getFaces().size()==10&&(std::find(outOfFrame.begin(),outOfFrame.end(),inFrame.at(u))==outOfFrame.end())){
			outOfFrame.push_back(inFrame.at(u));
		}
		inFrame.at(u)->increaseUnSeenCount();
		if (inFrame.at(u)->unSeenToLong()){
			inFrame.at(u)->outOfFrame(); //Mark the frame list inactive.
			inFrame.erase(inFrame.begin()+u);
		}

	}
}


// Internal helper function to to display the list of faces currently tracking
void FaceDetect::showFaces(std::vector<FaceList*> faces,cv::Mat rawFrame){

	//Initialize the big picture.
	cv::Mat bigPicture(480,4*98+10+640,CV_8UC3,0.0);
	rawFrame.copyTo(bigPicture(Rect(4*98+10,0,640,480)));
	rectangle(bigPicture,Point(98*4,0),Point(98*4+10,120),Scalar(255,0,0),CV_FILLED);
	rectangle(bigPicture,Point(98*4,120),Point(98*4+10,240),Scalar(0,255,0),CV_FILLED);
	rectangle(bigPicture,Point(98*4,240),Point(98*4+10,360),Scalar(0,0,255),CV_FILLED);
	rectangle(bigPicture,Point(98*4,360),Point(98*4+10,480),Scalar(0,255,255),CV_FILLED);

	cv::putText(
			bigPicture,std::string("D.Time:")+ std::to_string(detectionTime/((double)cvGetTickFrequency()*1000.))+std::string("ms"),
			Point(4*98+10+10,35),FONT_HERSHEY_SIMPLEX,0.75,Scalar::all(255),2);

	//Draw the tiny faces
	for (int i =3;i>=0;i--){
		for (int u =3;u>=0;u--){
			//std::cout << "i:" << i <<",u:"<<u << ",Size faces:"<<faces.size()<<std::endl;
			if (( (int)faces.size()-i )>0){
				vector<Face> list =faces.at(faces.size()-1-i)->getFaces();
				//std::cout <<"        Size faceList:"<<list.size()<<std::endl;
				if (((int)list.size()-u)>0){
					Face face=list.at(list.size()-1-u);

					//Resize and recolor face picture.
					cv::Mat faceRes,faceCol;
					cvtColor( face.getFace(), faceCol, CV_GRAY2BGR );
					cv::resize( faceCol, faceRes, Size(98,120), 0, 0, INTER_LINEAR );

					//Draw text on small picture
					cv::putText(
							faceRes,std::to_string(list.size()-1-u),
							Point(0,16),FONT_HERSHEY_SIMPLEX,0.5,Scalar::all(255),1.5);
					cv::putText(
							faceRes,std::to_string(faces.at(faces.size()-1-i)->getUid()),
							Point(0,118),FONT_HERSHEY_SIMPLEX,0.5,Scalar::all(0),2);

					// Add small picture to big.
					faceRes.copyTo(bigPicture(Rect((3-u)*98,(i)*120,98,120)));
				}

				//Pick color for face.
				Scalar color(255,255,255);
				if (i==0) color=Scalar(255,0,0);
				if (i==1) color=Scalar(0,255,0);
				if (i==2) color=Scalar(0,0,255);
				if (i==3) color=Scalar(0,255,255);

				//Draw tracking-lines on bigpicture
				Point lastPoint=Point(list.back().getPoint().x+4*98+10,list.back().getPoint().y);
				for (uint i=0;i<50;i++){
					if ((list.size()-1)<=i)
						break;
					else{
						Point currentPoint=list.at(list.size()-i-2).getPoint();
						currentPoint=Point(currentPoint.x+4*98+10,currentPoint.y);
						line(bigPicture,lastPoint,currentPoint,color,2);
						lastPoint=currentPoint;
					}
				}

				//Draw circle around head
				Point circlePoint=Point(list.back().getPoint().x+4*98+10,list.back().getPoint().y);
				circle(bigPicture, circlePoint, list.back().getRadius()*0.7, color, 2);
			}
		}
	}
	cv::imshow( "Big Picture", bigPicture );
}

void FaceDetect::showFaces2(std::vector<Face> faces){
	cv::Mat bigPicture(448,816,CV_8UC1,0.0);
	for (int i =0;i<faces.size();i++){
		Face face=faces.at(i);
		cv::Mat faceRes;
		cv::resize( face.getFace(), faceRes, Size(92,112), 0, 0, INTER_LINEAR );
		//cv::imshow("face",faceRes);
		faceRes.copyTo(bigPicture(Rect((i)*92,0,92,112)));//
	}
	cv::imshow( "Small Picture", bigPicture );
}



